#include "./PORT/port.h"
#include "../System/config.h"

#include "./BSP/JYxx/jy60.h"
#include "./BSP/JYxx/jy90.h"
#include "./BSP/JYxx/jyxx.h"
#include "./BSP/DEBUG/debug.h"

/**
 * @brief       JYxx初始化
 * @param       无
 * @retval      无
 */
void jyxx_init(void)
{
#if CFG_JYxx_MODULE ==  1
    jy60_init();
#elif CFG_JYxx_MODULE == 2
    jy901s_init();
#endif
}

/**
 * @brief       设置JYxx休眠或唤醒
 * @param       state: 休眠或唤醒   1:休眠   0:唤醒
 * @retval      无
 */
void jyxx_set_sleep_wake(uint8_t state)
{
#if CFG_JYxx_MODULE == 1
    jy60_set_sleep_wake(state);
#elif CFG_JYxx_MODULE == 2
    
#endif
}

/**
 * @brief       更新JYxx数据
 * @param       无
 * @retval      无
 */
void jyxx_up_data(void)
{
#if CFG_JYxx_MODULE == 1
    jy60_up_data();
#elif CFG_JYxx_MODULE == 2
    jy901s_up_data();
#endif
}

/**
 * @brief       获取JYxx加速度
 * @param       加速度
 * @retval
 */
float *jyxx_get_acceleration(void)
{
#if CFG_JYxx_MODULE == 1
    return jy60_get_acceleration();
#elif CFG_JYxx_MODULE == 2
    return jy901s_get_acceleration();
#endif
}

/**
 * @brief       获取JYxx角速度
 * @param       无
 * @retval      角速度
 */
float *jyxx_get_angle_speed(void)
{
#if CFG_JYxx_MODULE == 1
    return jy60_get_angle_speed();
#elif CFG_JYxx_MODULE == 2
    return jy901s_get_angle_speed();
#endif
}

/**
 * @brief       获取JYxx角度
 * @param       无
 * @retval      角度
 */
float *jyxx_get_angle(void)
{
#if CFG_JYxx_MODULE == 1
    return jy60_get_angle();
#elif CFG_JYxx_MODULE == 2
    return jy901s_get_angle();
#endif
}

/**
 * @brief       获取JYxx温度
 * @param       无
 * @retval      温度
 */
float jyxx_get_temperature(void)
{
#if CFG_JYxx_MODULE == 1
    return jy60_get_temperature();
#elif CFG_JYxx_MODULE == 2
    return 0;
#endif
}

/**
 * @brief       获取JYxx电压
 * @param       无
 * @retval      电压
 */
float jyxx_get_voltage(void)
{
#if CFG_JYxx_MODULE == 1
    return jy60_get_voltage();
#elif CFG_JYxx_MODULE == 2
    return 0;
#endif
}

/**
 * @brief       获取JYxx版本
 * @param       无
 * @retval      版本
 */
float jyxx_get_version(void)
{
#if CFG_JYxx_MODULE == 1
    return jy60_get_version();
#elif CFG_JYxx_MODULE == 2
    return 0;
#endif
}

/**
 * @brief 解缠绕偏航角，消除±180°跳变
 * @param current_yaw 当前帧的偏航角（-180~180）
 * @return 连续变化的偏航角
 */
float jyxx_unwrap_yaw(float current_yaw)
{
    // 计算与上一帧的角度差
    static float prev_yaw = 0.0f;
    float delta = current_yaw - prev_yaw;

    // 处理角度跳变
    if (delta > 180.0f)
    {
        delta -= 360.0f;
    }
    else if (delta < -180.0f)
    {
        delta += 360.0f;
    }

    // 更新连续角度
    float unwrapped_yaw = prev_yaw + delta;
    prev_yaw = unwrapped_yaw;

    if (unwrapped_yaw >= 360 || unwrapped_yaw <= -360)
    {
        unwrapped_yaw = 0;
    }

    return unwrapped_yaw;
}

/**
 * @brief       JYxx数据在OLED上显示
 * @param       无
 * @retval      无
 */
void jyxx_oled_show(void)
{
#include "./BSP/OLED/OLED.h"
    float *Axyz = jyxx_get_acceleration();
    // OLED_Printf(0, 8 * 2, OLED_6X8, "JSD:%4.1f %4.1f %4.1f        ", Axyz[0], Axyz[1], Axyz[2]);
    debug_printf("JSD:%4.1f %4.1f %4.1f        ", Axyz[0], Axyz[1], Axyz[2]);
    float *Wxyz = jyxx_get_angle_speed();
    // OLED_Printf(0, 8 * 3, OLED_6X8, "JD :%4.1f %4.1f %4.1f        ", Wxyz[0], Wxyz[1], Wxyz[2]);
    debug_printf("JD :%4.1f %4.1f %4.1f        ", Wxyz[0], Wxyz[1], Wxyz[2]);
    float *Grpy = jyxx_get_angle();
    // OLED_Printf(0, 8 * 4, OLED_6X8, "OLJ:%4.1f %4.1f %4.1f        ", Grpy[0], Grpy[1], Grpy[2]);
    debug_printf("OLJ:%4.1f %4.1f %4.1f        ", Grpy[0], Grpy[1], Grpy[2]);
}
